﻿using Kimd.Logic;
using Kimd.Machine;
using Kimd.Motion;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.ExplorerBar;
using System.Threading;
using WorkStation.Common;
using System.Linq;
using System.Collections.Generic;
using Kimd.Common;

namespace WorkStation.Logic
{
    /// <summary>
    /// 下料逻辑
    /// </summary>
    internal class UnloadTesterLogic : FlowBase
    {
        private UnloadTesterStatus _status;
        private Axis _x;
        private Axis _z;
        private Axis _r;
        private Axis _y;
        private Axis _ngY;
        private Tray _bufferTray;
        //cylinder
        private List<Cylinder> _nozzles;
        private List<Cylinder> _vacs;
        private List<Cylinder> _revVacs;
        private Cylinder _changeCylinder;
        //sensor
        private List<Sensor> _vacSense;
        private Tray _nozzleTray;
        private Tray _testerTray;
        private Tray _okTray;
        private TesterStatusLogic[] _testerStatusLogics; 
        private Dictionary<int, (List<int>, List<int>)> _getIndex2Nozzles;
        public UnloadTesterLogic(string flowName, FlowManager flowmanager)
            : base(flowName, flowmanager)
        {
            _x = MachineResource.Instance.MachineAxis["下料移载X轴"];
            _y = MachineResource.Instance.MachineAxis["下料仓Y轴"];
            _z = MachineResource.Instance.MachineAxis["下料移载Z轴"];
            _r = MachineResource.Instance.MachineAxis["下料移载U轴"];
            _ngY = MachineResource.Instance.MachineAxis["NG下料区Y轴"];

            _nozzles = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["下料移载取料气缸1下降V"],
                MachineResource.Instance.MachineCylinder["下料移载取料气缸2下降V"],
                MachineResource.Instance.MachineCylinder["下料移载取料气缸3下降V"],
                MachineResource.Instance.MachineCylinder["下料移载取料气缸4下降V"],
            };
            _vacs = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["下料移载取料1真空吸"],
                MachineResource.Instance.MachineCylinder["下料移载取料2真空吸"],
                MachineResource.Instance.MachineCylinder["下料移载取料3真空吸"],
                MachineResource.Instance.MachineCylinder["下料移载取料4真空吸"],
            };
            _revVacs = new List<Cylinder>()
            {
                MachineResource.Instance.MachineCylinder["上料移载取料1破真空"],
                MachineResource.Instance.MachineCylinder["上料移载取料2破真空"],
                MachineResource.Instance.MachineCylinder["上料移载取料3破真空"],
                MachineResource.Instance.MachineCylinder["上料移载取料4破真空"],
            };
            _changeCylinder = MachineResource.Instance.MachineCylinder["下料移栽变距气缸动点V"];
            _vacSense = new List<Sensor>()
            {
                MachineResource.Instance.MachineSensor["下料移载取料1真空达到"],
                MachineResource.Instance.MachineSensor["下料移载取料2真空达到"],
                MachineResource.Instance.MachineSensor["下料移载取料3真空达到"],
                MachineResource.Instance.MachineSensor["下料移载取料4真空达到"],
            };

            _bufferTray = new Tray(1, 4);
            _nozzleTray = new Tray(1, 4);
            _testerTray = new Tray(1, 4);
            _okTray = new Tray(1, 10);

            _testerStatusLogics = new TesterStatusLogic[2];
            _testerStatusLogics[0] = FlowManager.Instance.SelectFlow("测试机状态1") as TesterStatusLogic;
            _testerStatusLogics[1] = FlowManager.Instance.SelectFlow("测试机状态2") as TesterStatusLogic;

            _getIndex2Nozzles = new Dictionary<int, (List<int>, List<int>)>();
            _getIndex2Nozzles.Add(1, (new List<int>() { 1, 2, 3, 4 }, new List<int>()));
            _getIndex2Nozzles.Add(2, (new List<int>() { 5 }, new List<int>() { 6, 7, 8 }));
            _getIndex2Nozzles.Add(3, (new List<int>() { 9, 10 }, new List<int>() { 11, 12 }));
            _getIndex2Nozzles.Add(4, (new List<int>() { 13, 14, 15 }, new List<int>() { 16 }));
            _getIndex2Nozzles.Add(5, (new List<int>(), new List<int>() { 17, 18, 19, 20 }));

            RegisterDirectSignal($"取测试治具1完成");
            RegisterDirectSignal($"取测试治具2完成");
            RegisterDirectSignal("料放完");

            RegisterDirectSignal("Z轴已抬起");
            RegisterDirectSignal("回零完成");
        }

        public override void Calibrate()
        {
            base.Calibrate();
        }

        public override void Home()
        {
            if (!CheckFlowCondition()) return;
            MultiCylinderOperate(_nozzles, 5000, new List<bool>() { false, false, false, false });
            if (!CheckFlowCondition()) return;
            SingleCylinderOperate(_changeCylinder, 5000, false);
            if (!CheckFlowCondition()) return;

            _revVacs.ForEach(v => v.Off());
            for (int i = 0; i < _vacs.Count; i++)
            {
                if (!_vacSense[i].WaitOn())
                {
                    _vacs[i].Off();
                }
            }

            _z.ClearAlarm();
            Thread.Sleep(200);
            _z.SetServoOn();
            Thread.Sleep(200);
            _x.ClearAlarm();
            Thread.Sleep(200);
            _x.SetServoOn();
            Thread.Sleep(200);
            _r.ClearAlarm();
            Thread.Sleep(200);
            _r.SetServoOn();
            Thread.Sleep(200);
            _ngY.ClearAlarm();
            Thread.Sleep(200);
            _ngY.SetServoOn();
            Thread.Sleep(200);

            if (!GlobalVariable.IsSystemAlreadyHome)
                AxisGoHomeSimultaneously(new List<Axis>() { _x, _ngY });
            else
                AxisMoveSimultaneously(new List<Axis>() { _x, _ngY }, new List<double>() { 0, 0 });
            if (!CheckFlowCondition()) return;

            if (!GlobalVariable.IsSystemAlreadyHome)
                SingleAxisGoHome(_r);
            else
                SingleAxisMove2Point(_r, 0);
            if (!CheckFlowCondition()) return;
            SingleAxisMove2Point(_r, "待机位");
            if (!CheckFlowCondition()) return;

            if (!GlobalVariable.IsSystemAlreadyHome)
                SingleAxisGoHome(_z);
            else
                SingleAxisMove2Point(_z, 0);
            if (!CheckFlowCondition()) return;
            SetSignal("Z轴已抬起");

            SingleAxisMove2Point(_z, "R轴回零位");
            if (!CheckFlowCondition()) return;

            checkProductStatus();

            NeedReleaseTester1 = false;

            SetSignal("回零完成");

            _okTray.Helper.NewFullTray();
        }

        public override void Init(out string errInfo)
        {
            base.Init(out errInfo);
        }

        public override void InSingleRunMode()
        {
            base.InSingleRunMode();
        }

        bool NeedReleaseTester1 = false;
        public override void Method()
        {
            if (!CheckFlowCondition()) return;

            int finishTesterIndex = SelectFinishTester();
            if (finishTesterIndex != -1)
            {
                switch (finishTesterIndex)
                {
                    case 0:
                        //请求进入tester1区域去下料
                        Monitor.Enter(GlobalVariable.Tester1AreaLock);
                        GoAndGetProduct(0);
                        NeedReleaseTester1 = true;
                        break;
                    case 1:
                        //请求进入tester2区域去下料
                        Monitor.Enter(GlobalVariable.Tester1AreaLock);
                        GoAndGetProduct(1);
                        NeedReleaseTester1 = true;
                        break;
                }
                //接下来判断什么时候 释放锁
                //有NG就到NG放料位，并放料
                _changeCylinder.Off();
                if (_nozzleTray.Helper.Any(c => c.Result == TestResult.FirstTimeNG))
                {
                    int[] ngIndex = _nozzleTray.Helper.Select((c, index) => new { Cell = c, Index = index })
                                                                        .Where(item => item.Cell.Status == CellStatus.HasProduct && item.Cell.Result == TestResult.FirstTimeNG)
                                                                        .Select(item => item.Index).ToArray();
                    //移动前准备
                    prepareMove();
                    if (!CheckFlowCondition()) return;
                    //ReleaseLock();
                    lock (GlobalVariable.NGTrackLock)
                    {
                        if (NeedReleaseTester1)
                        {
                            NeedReleaseTester1 = false;
                            int putIndex = GlobalVariable.NGTray.Helper.GetFirstIndex(CellStatus.NoProduct);
                            if (putIndex == -1) putIndex = 0;
                            AxisMoveSimultaneously(_x, _ngY, $"{"放料NG矩阵"}-{putIndex}-{ngIndex[0]}", lockObj: GlobalVariable.Tester1AreaLock);
                        }
                    }
                    if (!CheckFlowCondition()) return;
                    SingleCylinderOperate(_changeCylinder, 5000, false);
                    PutNGProduct(ngIndex);
                    if (!CheckFlowCondition()) return;
                }
                int okCount = _nozzleTray.Helper.Where(c => c.Status == CellStatus.HasProduct).Count();
                //至少手上剩下的料要大于0个吧
                if (okCount > 0)
                {
                    if (okCount < 4)
                    {
                        //先判断是放buffer还是取buffer
                        int bufferCount = _bufferTray.Helper.Where(c => c.Status == CellStatus.HasProduct).Count();
                        //buffer上的料够补齐手上的4个
                        if (bufferCount >= (4 - okCount))
                        {
                            //去第一个可取的buffer位置，释放锁
                            int noProductIndex = _nozzleTray.Helper.GetFirstIndex(CellStatus.NoProduct);
                            int hasProductIndex = _bufferTray.Helper.GetFirstIndex(CellStatus.HasProduct);
                            //移动前准备
                            prepareMove();
                            if (!CheckFlowCondition()) return;
                            //如果前面没有NG过，前面没有释放锁，需要在这里释放
                            //ReleaseLock();
                            if (NeedReleaseTester1)
                            {
                                NeedReleaseTester1 = false;
                                AxisMoveSimultaneously(_x, _y, $"{"缓存矩阵"}-{hasProductIndex}-{noProductIndex}",lockObj: GlobalVariable.Tester1AreaLock);
                            }
                            if (!CheckFlowCondition()) return;
                            //取buffer 凑够4个去下料
                            SingleCylinderOperate(_changeCylinder, 5000, false);
                            GetBuffer();
                        }
                        else
                        {
                            //去第一个可放的buffer位置，释放锁
                            int noProductIndex = _bufferTray.Helper.GetFirstIndex(CellStatus.NoProduct);
                            int hasProductIndex = _nozzleTray.Helper.GetFirstIndex(CellStatus.HasProduct);
                            //移动前准备
                            prepareMove();
                            if (!CheckFlowCondition()) return;
                            //如果前面没有NG过，前面没有释放锁，需要在这里释放
                            //ReleaseLock();
                            if (NeedReleaseTester1)
                            {
                                NeedReleaseTester1 = false;
                                AxisMoveSimultaneously(_x, _y, $"{"缓存矩阵"}-{noProductIndex}-{hasProductIndex}", lockObj: GlobalVariable.Tester1AreaLock);
                            }
                            if (!CheckFlowCondition()) return;
                            //放完buffer后，重新开始
                            SingleCylinderOperate(_changeCylinder, 5000, false);
                            PutBuffer();
                        }
                    }
                    //还要判断一次手上OK的料的个数，可能是取完buffer以后过来的
                    okCount = _nozzleTray.Helper.Where(c => c.Status == CellStatus.HasProduct).Count();
                    if (okCount > 0)
                    {
                        //移动前准备
                        prepareMove();
                        if (!CheckFlowCondition()) return;
                        //如果是4个OK的情况 或者 前面没有NG过，前面没有释放锁，需要在这里释放
                        //ReleaseLock();
                        if (NeedReleaseTester1)
                        {
                            NeedReleaseTester1 = false;

                            List<int> first = new List<int>();
                            List<int> second = new List<int>();
                            (first, second) = ChooseNozzles(out int putIndex0);
                            if (first.Count == 0 && second.Count == 0)
                            {
                                List<double> pos = new List<double>();
                                pos.Add(_x.MotionPosition[$"下料矩阵吸嘴1-0-0"]);
                                pos.Add(_r.Position);
                                AxisMoveSimultaneously(new List<Axis>() { _x, _r }, pos, lockObj: GlobalVariable.Tester1AreaLock);
                            }
                            else
                            {
                                if (GetNozzleStatusAndPosition(first, second,
                                                               out nozzleStatusFirst, out nozzleStatusSecond,
                                                               out posFirst, out posSecond,
                                                               out zGetPosFirst, out zGetPosSecond))
                                {
                                    if (!string.IsNullOrEmpty(zGetPosFirst))
                                    {
                                        List<double> pos = new List<double>();
                                        pos.Add(_x.MotionPosition[zGetPosFirst]);
                                        pos.Add(_r.Position);
                                        AxisMoveSimultaneously(new List<Axis>() { _x, _r }, pos, lockObj: GlobalVariable.Tester1AreaLock);
                                    }
                                    else
                                    {
                                        List<double> pos = new List<double>();
                                        pos.Add(_x.MotionPosition[zGetPosSecond]);
                                        pos.Add(_r.Position);
                                        AxisMoveSimultaneously(new List<Axis>() { _x, _r }, pos, lockObj: GlobalVariable.Tester1AreaLock);
                                    }
                                }
                                else
                                {

                                }
                            }
                        }
                        if (!CheckFlowCondition()) return;


                        while (_okTray.Helper.IsFull && !IsBreak())
                        {
                            Thread.Sleep(20);
                        }
                        if (!CheckFlowCondition()) return;

                        if (okCount == 4)
                        {
                            SingleCylinderOperate(_changeCylinder, 5000, false);
                            //此时可以安心下料
                            PutOKProduct();
                            if (isSimulate)
                                Thread.Sleep(500);
                        }
                        else
                        {
                            //清料的情况下会发生

                        }
                    }
                }
            }
        }

        public override void OnStop()
        {
            base.OnStop();
        }

        public override bool PreStartCheck()
        {
            return base.PreStartCheck();
        }

        public override void SpotCheck()
        {
            base.SpotCheck();
        }
        public int SelectFinishTester()
        {
            if (GlobalVariable.IsDryRun)
            {
                int idleTesterIndex = -1;
                List<TesterStatus> status = new List<TesterStatus>();
                status.Add((_flowManager.SelectFlow($"测试治具1") as SimTesterLogic).CurrentTesterStatus);
                status.Add((_flowManager.SelectFlow($"测试治具2") as SimTesterLogic).CurrentTesterStatus);
                List<bool> inUse = new List<bool>();
                inUse.Add(SystemConfig.Instance.UsingStatus.TesterInUseStatus[0]);
                inUse.Add(SystemConfig.Instance.UsingStatus.TesterInUseStatus[1]);

                //for (int i = status.Count - 1; i >= 0; i--)
                //{
                //    //只要有1个tester启用了，就认为整个模组启用
                //    if (inUse[i])
                //    {
                //        if (status[i] == TesterStatus.TestFinish)
                //        {
                //            idleTesterIndex = i;
                //            break;
                //        }
                //    }
                //}
                for (int i = 0; i < 2; i++)
                {
                    //只要有1个tester启用了，就认为整个模组启用
                    if (inUse[i])
                    {
                        if (status[i] == TesterStatus.TestFinish)
                        {
                            idleTesterIndex = i;
                            break;
                        }
                    }
                }
                return idleTesterIndex;
            }
            else
            {
                for (int i = 0; i < 2; i++)
                {
                    if (!SystemConfig.Instance.UsingStatus.TesterInUseStatus[i])
                        continue;
                    isTesterAlarm(i);
                    TesterStatusLogic testerLogic = (FlowManager.Instance.SelectFlow($"测试机状态{i + 1}") as TesterStatusLogic);
                    if (testerLogic.IsInAction(TesterActionInFlag.转盘允许取料))
                        return i;
                }
            }
            return -1;
        }
        public void GoAndGetProduct(int testerIndex)
        {
            SingleAxisMove2Point(_r, $"治具{testerIndex + 1}取料位");
            if (!CheckFlowCondition()) return;
            _changeCylinder.On();
            AxisMoveSimultaneously(_x, _r, $"治具{testerIndex + 1}取料位");
            if (!CheckFlowCondition()) return;
            SingleCylinderOperate(_changeCylinder, 5000, true);
            if (!CheckFlowCondition()) return;

            _testerTray.Helper.NewEmptyTray();
            if (!GlobalVariable.IsDryRun)
            {
                for (int i = 0; i < 4; i++)
                {
                    TesterCellScanInFlag scanFlag = _testerStatusLogics[testerIndex].GetCellScanStatus(i);
                    switch(scanFlag)
                    {
                        case TesterCellScanInFlag.无产品:
                            _testerTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                            break;
                        case TesterCellScanInFlag.流程错误:
                        case TesterCellScanInFlag.无数据:
                        case TesterCellScanInFlag.条码格式错误:
                        case TesterCellScanInFlag.扫码异常:
                            _testerTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                            _testerTray.Helper.SetCellResult(i, TestResult.FirstTimeNG);
                            break;
                    }

                    TesterCellResultInFlag resultFlag = _testerStatusLogics[testerIndex].GetCellResultStatus(i);
                    switch (resultFlag)
                    {
                        case TesterCellResultInFlag.无产品:
                            _testerTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                            break;
                        case TesterCellResultInFlag.测试PASS:
                            _testerTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                            _testerTray.Helper.SetCellResult(i, TestResult.FirstTimeOK);
                            break;
                        case TesterCellResultInFlag.测试FAIL:
                            _testerTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                            _testerTray.Helper.SetCellResult(i, TestResult.FirstTimeNG);
                            break;
                    }
                }

                //交换产品状态
                for (int i = 0; i < 4; i++)
                {
                    if (_testerTray.Helper.GetCell(i).Status == CellStatus.HasProduct)
                    {
                        _nozzleTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                        _nozzleTray.Helper.SetCellResult(i, _testerTray.Helper.GetCell(i).Result);
                        _testerTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                    }
                    else
                        _nozzleTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                }
            }
            else
            {
                //for (int i = 0; i < 4; i++)
                //{
                //    _testerTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                //    _testerTray.Helper.SetCellResult(i, TestResult.FirstTimeOK);
                //}
                Tray t = (FlowManager.Instance.SelectFlow($"测试治具{testerIndex + 1}") as SimTesterLogic).TesterTray;
                for (int i = 0; i < 4; i++)
                {
                    _testerTray.Helper.SetCellStatus(i, t.Helper.GetCell(i).Status);
                    _testerTray.Helper.SetCellBarcode(i, t.Helper.GetCell(i).SN);
                    _testerTray.Helper.SetCellResult(i, t.Helper.GetCell(i).Result);
                }
            }
            NozzleGetProductExt(_z, _nozzles, _vacs, _vacSense, null, null,
                                $"治具{testerIndex + 1}取料位", "安全位置", "",
                                5000, delayAfterOpenVac: 100, delayBeforeVacCheck: 100,
                                GlobalVariable.IsDryRun,
                                _testerTray.Helper.Select(c => c.Status == CellStatus.HasProduct).ToList());
            

            if(!GlobalVariable.IsDryRun)
            {
                isTesterAlarm(testerIndex);
                //给出上下料机取料完成信号
                CommonMethod.SetOnoff(ref GlobalVariable.HandlerActionData[testerIndex], (int)HandlerActionOutFlag.上下料机取料完成, true);
                //等待转盘的允许取料信号灭掉
                while (_testerStatusLogics[testerIndex].IsInAction(TesterActionInFlag.转盘允许取料) && !IsBreak())
                {
                    Thread.Sleep(20);
                }
                if (!CheckFlowCondition()) return;
                //关闭上下料机取料完成信号
                CommonMethod.SetOnoff(ref GlobalVariable.HandlerActionData[testerIndex], (int)HandlerActionOutFlag.上下料机取料完成, false);
            }
            else
            {
                Tray testerTray = (FlowManager.Instance.SelectFlow($"测试治具{testerIndex + 1}") as SimTesterLogic).TesterTray;
                for (int i = 0; i < 4; i++)
                {
                    if(testerTray.Helper.GetCell(i).Status == CellStatus.HasProduct)
                    {
                        _nozzleTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                        _nozzleTray.Helper.SetCellBarcode(i, testerTray.Helper.GetCell(i).SN);
                        _nozzleTray.Helper.SetCellResult(i, testerTray.Helper.GetCell(i).Result);

                        testerTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                    }
                    else
                        _nozzleTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                }
                SetSignal($"取测试治具{testerIndex + 1}完成");
            }
        }
        public void isTesterAlarm(int testerIndex)
        {
            TesterStatusLogic testerStatus = FlowManager.Instance.SelectFlow($"测试机状态{testerIndex + 1}") as TesterStatusLogic;
            while (testerStatus.IsInStatus(TesterStatusInFlag.转盘报警中) && !IsBreak())
            {
                ShowWarnAndPause($"治具{testerIndex + 1}报警中，请检查", _x.Module);
                if (!CheckFlowCondition()) return;
                Thread.Sleep(20);
            }
            if (!CheckFlowCondition()) return;
        }
        public void prepareMove()
        {
            SingleAxisMove2Point(_z, "安全位置");
            if (!CheckFlowCondition()) return;
        }
        public void ReleaseLock()
        {
            if (NeedReleaseTester1)
            {
                NeedReleaseTester1 = false;
                Monitor.Exit(GlobalVariable.Tester1AreaLock);
            }
        }
        public void PutNGProduct(int[] ngIndex)
        {
            checkTrayFullAndReplace(GlobalVariable.NGTray, "NG Tray已满,请取出放置新的Tray盘", "下料逻辑", GlobalVariable.IsDryRun);
            int putIndex = GlobalVariable.NGTray.Helper.GetFirstIndex(CellStatus.NoProduct);
            SingleAxisMove2Point(_z, "安全位置");
            if (!CheckFlowCondition()) return;
            for (int i = 0; i < ngIndex.Length; i++)
            {
                putIndex = GlobalVariable.NGTray.Helper.GetFirstIndex(CellStatus.NoProduct);
                AxisMoveSimultaneously(_x, _ngY, _r, $"放料NG矩阵-{putIndex}-{ngIndex[i]}");
                if (!CheckFlowCondition()) return;
                NozzlePutProduct(_nozzles[ngIndex[i]], _vacs[ngIndex[i]], _revVacs[ngIndex[i]], 5000, delayBeforeCloseVac: 0, delayBeforeNozzleUp: 100);
                if (!CheckFlowCondition()) return;
                _nozzleTray.Helper.SetCellStatus(ngIndex[i], CellStatus.NoProduct);
                GlobalVariable.NGTray.Helper.SetCellStatus(putIndex, CellStatus.HasProduct);
            }
        }
        public void GetBuffer()
        {
            for (int i = 0; i < 4; i++)
            {
                if (_nozzleTray.Helper.GetCell(i).Status == CellStatus.NoProduct)
                {
                    int hasProductIndex = _bufferTray.Helper.GetFirstIndex(CellStatus.HasProduct);
                    lock (GlobalVariable.NGTrackLock)
                    {
                        AxisMoveSimultaneously(_x, _ngY, _r, $"{"缓存矩阵"}-{hasProductIndex}-{i}");
                    }
                    if (!CheckFlowCondition()) return;
                    NozzleGetProductExt(_z, new List<Cylinder>() { _nozzles[i] }, new List<Cylinder>() { _vacs[i] }, new List<Sensor>() { _vacSense[i] }, null, null,
                                        $"{"缓存矩阵"}-{hasProductIndex}-{i}", "安全位置", "",
                                        5000, delayAfterOpenVac: 100, delayBeforeVacCheck: 100,
                                        GlobalVariable.IsDryRun,
                                        new List<bool>() { true });
                    if (!CheckFlowCondition()) return;
                    //交换产品状态
                    _nozzleTray.Helper.SetCellStatus(i, CellStatus.HasProduct);
                    _nozzleTray.Helper.SetCellBarcode(i, _bufferTray.Helper.GetCell(hasProductIndex).SN);
                    _nozzleTray.Helper.SetCellResult(i, _bufferTray.Helper.GetCell(hasProductIndex).Result);

                    _bufferTray.Helper.SetCellStatus(hasProductIndex, CellStatus.NoProduct);
                    _bufferTray.Helper.SetCellBarcode(hasProductIndex, string.Empty);
                    _bufferTray.Helper.SetCellResult(hasProductIndex, TestResult.NoTest);
                }
            }
        }
        public void PutBuffer()
        {
            for (int i = 0; i < 4; i++)
            {
                if (_nozzleTray.Helper.GetCell(i).Status == CellStatus.HasProduct)
                {
                    int hasProductIndex = _bufferTray.Helper.GetFirstIndex(CellStatus.NoProduct);
                    lock (GlobalVariable.NGTrackLock)
                    {
                        AxisMoveSimultaneously(_x, _ngY, _r, $"{"缓存矩阵"}-{hasProductIndex}-{i}");
                    }
                    if (!CheckFlowCondition()) return;
                    NozzlePutProductExt(_z,
                                        new List<Cylinder>() { _nozzles[i] }, new List<Cylinder>() { _vacs[i] }, new List<Cylinder>() { _revVacs[i] }, null,
                                        $"{"缓存矩阵"}-{hasProductIndex}-{i}", "安全位置", 0, 0,
                                        5000, delayBeforeNozzleUp: 100,
                                        new List<bool>() { true });
                    if (!CheckFlowCondition()) return;
                    //交换产品状态
                    _bufferTray.Helper.SetCellBarcode(hasProductIndex, _nozzleTray.Helper.GetCell(i).SN);
                    _bufferTray.Helper.SetCellResult(hasProductIndex, _nozzleTray.Helper.GetCell(i).Result);
                    _bufferTray.Helper.SetCellStatus(hasProductIndex, CellStatus.HasProduct);
                    GlobalVariable.BufferTray.Helper.SetCellBarcode(hasProductIndex, _nozzleTray.Helper.GetCell(i).SN);
                    GlobalVariable.BufferTray.Helper.SetCellResult(hasProductIndex, _nozzleTray.Helper.GetCell(i).Result);
                    GlobalVariable.BufferTray.Helper.SetCellStatus(hasProductIndex, CellStatus.HasProduct);

                    _nozzleTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                }
            }
        }

        List<bool> nozzleStatusFirst = null;
        List<bool> nozzleStatusSecond = null;
        List<double> posFirst = null;
        List<double> posSecond = null;
        string zGetPosFirst = "";
        string zGetPosSecond = "";
        public void PutOKProduct()
        {
            List<int> first = new List<int>();
            List<int> second = new List<int>();
            (first, second) = ChooseNozzles(out int getIndex);
            if (first.Count == 0 && second.Count == 0)
            {
                //放完了
                SetSignal("料放完");
            }
            else
            {
                if (GetNozzleStatusAndPosition(first, second,
                                               out nozzleStatusFirst, out nozzleStatusSecond,
                                               out posFirst, out posSecond,
                                               out zGetPosFirst, out zGetPosSecond))
                {
                    //Z轴抬起
                    SingleAxisMove2Point(_z, "安全位置");
                    if (!CheckFlowCondition()) return;
                    //第一list里面有取料点位，且视觉判定下来有需要取料的
                    if (posFirst.Count != 0 && nozzleStatusFirst.
                        Any(c => c) && !string.IsNullOrEmpty(zGetPosFirst))
                    {
                        posFirst.Add(_r.MotionPosition[zGetPosFirst]);
                        AxisMoveSimultaneously(new List<Axis>() { _x, _y, _r}, posFirst);
                        if (!CheckFlowCondition()) return;
                        NozzlePutProductExt(_z,
                                            _nozzles, _vacs, _revVacs, null,
                                            zGetPosFirst, "安全位置", 0, 0,
                                            5000, delayBeforeNozzleUp: 100,
                                            nozzleStatusFirst); 
                        if (!CheckFlowCondition()) return;

                        //交换料状态
                        for (int i = 0; i < nozzleStatusFirst.Count; i++)
                        {
                            if (nozzleStatusFirst[i])
                            {
                                _nozzleTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                            }
                        }
                    }
                    if (posSecond.Count != 0 && nozzleStatusSecond.Any(c => c) && !string.IsNullOrEmpty(zGetPosSecond))
                    {
                        posSecond.Add(_r.MotionPosition[zGetPosSecond]);
                        AxisMoveSimultaneously(new List<Axis>() { _x, _y, _r }, posSecond);
                        if (!CheckFlowCondition()) return;
                        NozzlePutProductExt(_z,
                                            _nozzles, _vacs, _revVacs, null,
                                            zGetPosSecond, "安全位置", 0, 0,
                                            5000, delayBeforeNozzleUp: 100,
                                            nozzleStatusSecond);
                        if (!CheckFlowCondition()) return;

                        //交换料状态
                        for (int i = 0; i < nozzleStatusSecond.Count; i++)
                        {
                            if (nozzleStatusSecond[i])
                                _nozzleTray.Helper.SetCellStatus(i, CellStatus.NoProduct);
                        }
                    }
                }
                if (getIndex != -1)
                    _okTray.Helper.SetCellStatus(getIndex, CellStatus.HasProduct);
                (first, second) = ChooseNozzles(out getIndex);
                if (first.Count == 0 && second.Count == 0)
                {
                    SetSignal("料放完");
                }
            }
        }
        public (List<int>, List<int>) ChooseNozzles(out int getIndex)
        {
            getIndex = _okTray.Helper.GetFirstIndex(CellStatus.NoProduct);
            //半盘还没取完的话
            if (getIndex != -1)
            {
                List<int> firstGet = new List<int>();
                List<int> secondGet = new List<int>();
                (firstGet, secondGet) = _getIndex2Nozzles[(getIndex % 5 + 1)];
                int index = 0;
                if (getIndex < 5)
                    index = 0;
                else if (getIndex >= 5)
                    index = 1;
                return (firstGet.Select(x => x + 20 * index).ToList(), secondGet.Select(x => x + 20 * index).ToList());
            }
            return (new List<int>(), new List<int>());
        }
        public bool GetNozzleStatusAndPosition(List<int> first, List<int> second,
                                               out List<bool> nozzleStatusFirst, out List<bool> nozzleStatusSecond,
                                               out List<double> posFirst, out List<double> posSecond,
                                               out string zGetPosFirst, out string zGetPosSecond)
        {
            //结合视觉看下哪几个吸嘴要下
            nozzleStatusFirst = new List<bool>();
            nozzleStatusFirst = first.Select(x => true).ToList();
            nozzleStatusSecond = new List<bool>();
            nozzleStatusSecond = second.Select(x => true).ToList();
            //前提是都有可取的料
            posFirst = new List<double>();
            posSecond = new List<double>();
            zGetPosFirst = "";
            zGetPosSecond = "";
            string matrixName1 = "下料矩阵吸嘴1";
            string matrixName4 = "下料矩阵吸嘴4";
            if (nozzleStatusFirst.Any(c => c) || nozzleStatusSecond.Any(c => c))
            {
                //选择first的点位
                int firstchange = first.Find(x => x % 20 == 1 || x % 20 == 6 || x % 20 == 11 || x % 20 == 16);
                if (firstchange != 0)
                {
                    posFirst.Add(_x.MotionPosition[$"{matrixName1}-{firstchange % 40 / 5}-{0}"]);
                    posFirst.Add(_y.MotionPosition[$"{matrixName1}-{firstchange % 40 / 5}-{0}"]);
                    zGetPosFirst = $"{matrixName1}-{firstchange % 40 / 5}-{0}";
                    int count = 4 - nozzleStatusFirst.Count;
                    for (int i = 0; i < count; i++)
                    {
                        nozzleStatusFirst.Add(false);
                    }
                }
                else
                {
                    firstchange = first.Find(x => x % 20 == 0 || x % 20 == 5 || x % 20 == 10 || x % 20 == 15 || x % 20 == 20);
                    if (firstchange != 0)
                    {
                        posFirst.Add(_x.MotionPosition[$"{matrixName4}-{(firstchange / 5 - 1) % 8}-{0}"]);
                        posFirst.Add(_y.MotionPosition[$"{matrixName4}-{(firstchange / 5 - 1) % 8}-{0}"]);
                        zGetPosFirst = $"{matrixName4}-{(firstchange / 5 - 1) % 8}-{0}";
                        int count = 4 - nozzleStatusFirst.Count;
                        for (int i = 0; i < count; i++)
                        {
                            nozzleStatusFirst.Insert(0, false);
                        }
                    }
                }
                int secondchange = second.Find(x => x % 20 == 1 || x % 20 == 6 || x % 20 == 11 || x % 20 == 16);
                if (secondchange != 0)
                {
                    posSecond.Add(_x.MotionPosition[$"{matrixName1}-{secondchange % 40 / 5}-{0}"]);
                    posSecond.Add(_y.MotionPosition[$"{matrixName1}-{secondchange % 40 / 5}-{0}"]);
                    zGetPosSecond = $"{matrixName1}-{secondchange % 40 / 5}-{0}";
                    int count = 4 - nozzleStatusSecond.Count;
                    for (int i = 0; i < count; i++)
                    {
                        nozzleStatusSecond.Add(false);
                    }
                }
                else
                {
                    secondchange = second.Find(x => x % 20 == 0 || x % 20 == 5 || x % 20 == 10 || x % 20 == 15 || x % 20 == 20);
                    if (secondchange != 0)
                    {
                        posSecond.Add(_x.MotionPosition[$"{matrixName4}-{(secondchange / 5 - 1) % 8}-{0}"]);
                        posSecond.Add(_y.MotionPosition[$"{matrixName4}-{(secondchange / 5 - 1) % 8}-{0}"]);
                        zGetPosSecond = $"{matrixName4}-{(secondchange / 5 - 1) % 8}-{0}";
                        int count = 4 - nozzleStatusSecond.Count;
                        for (int i = 0; i < count; i++)
                        {
                            nozzleStatusSecond.Insert(0, false);
                        }
                    }
                }
                return true;
            }
            return false;
        }

        public void checkProductStatus()
        {
        restart:
            for (int i = 0; i < _vacSense.Count; i++)
            {
                if (_vacSense[i].WaitOn())
                {
                    MachineMessageScheduler.Instance.Add(new MachineMessage(MessageType.InfoAndPause, $"下料头上有料，请取走", this.Description, 0, "系统"));
                    //出错，停下所有线程
                    _flowManager.Pause();
                    //若为报错，需重新检测启动信号
                    WaitForStart();
                    if (!CheckFlowCondition()) return;
                    goto restart;
                }
            }
            //关闭所有真空
            _vacs.ForEach(v => v.Off());
        }
        public void NewOKTray()
        {
            _okTray.Helper.NewEmptyTray();
        }
    }
    public enum UnloadTesterStatus
    {
        Wait,
        UnloadTester,
    }
}
